#include <pololu/3pi.h>
#include <math.h>
#include "commands.h"

#define ROBOT_PERIMETER 9,4*M_PI 
#define ROBOT_CM_PER_SECOND 10
#define ROBOT_DISTANCE_ONE_DEGREE 0.08201302155121355
#define ROBOT_SPEEDSTEP 2.55
#define ROBOT_ROTATESPEED_IN_CMS 50
#define ROBOT_ROTATE_DAMPING 0.9

#ifndef F_CPU2
// Define the clock speed of the CPU
#define F_CPU2 20000000
#endif
#ifndef BAUDRATE
// Define the Baudrate for the USART
#define BAUDRATE 38400
#endif

#define UBRR_VAL ((F_CPU2+BAUDRATE*8)/(BAUDRATE*16)-1)
#define BAUD_REAL (F_CPU2/(16*(UBRR_VAL+1)))     // Real Baudrate
#define BAUD_ERROR ((BAUD_REAL*1000)/BAUDRATE) // Error in per mille

// A global ring buffer for data coming in.  This is used by the
// read_next_byte() and previous_byte() functions, below.
char buffer[100];

// A pointer to where we are reading from.
unsigned char read_index = 0;

uint16_t sensors[5];

void init_USART() {
	// Enable the receiver and transmitter
	UCSR0B = (1 << RXEN0 | 1 << TXEN0);
	
	// Define the USART frame.
	// We want 8 data bits, no parity bit and one stop bit
	UCSR0C = (1 << UCSZ00 | 1 << UCSZ01 | 0 << UPM01 | 0 << USBS0);
	
	// Initialize the UBRR register to achieve the desired BAUDRATE.
	// We assume normal asynchronous mode (thats why we divide by 16)
	UBRR0 = UBRR_VAL;
}

uint8_t receive_byte_USART(uint8_t* byte) {
	while (!(UCSR0A & (1<<RXC0))) {
		; // Wait till there is unread data in the receive buffer
	}
	
	// Check for errors
	if(UCSR0A & (1 << FE0) || UCSR0A & (1 << DOR0) || UCSR0A & (1 << UPE0)) {
		print("USART ER");
		*byte = UDR0;
		return -1;
	}
	
	*byte = UDR0;
	return 0;
}

char read_next_byte()
{
	while(serial_get_received_bytes() == read_index)
	{
		;
	}
	char ret = buffer[read_index];
	read_index ++;
	if(read_index >= 100)
	read_index = 0;
	return ret;
}

void get_RS_sensor_data()
{
	read_line_sensors(sensors, IR_EMITTERS_ON);
	serial_send_blocking((char *)sensors, 10);
}

void move_forward(){
	char speed = read_next_byte();
	set_motors(speed, speed);
}

void move_backward(){
	char speed = read_next_byte();
	set_motors(-speed, -speed);
}

void rotate(){
	/*uint16_t degree = read_next_byte() << 8;
	degree |= read_next_byte();
	
	set_motors(ROBOT_ROTATESPEED_IN_CMS*ROBOT_SPEEDSTEP+1, -ROBOT_ROTATESPEED_IN_CMS*ROBOT_SPEEDSTEP);
	delay_ms(ROBOT_ROTATE_DAMPING*(255/(ROBOT_SPEEDSTEP*ROBOT_ROTATESPEED_IN_CMS))*degree * ROBOT_DISTANCE_ONE_DEGREE * 10);
	set_motors(0,0);
	*/
	
	uint16_t degree = read_next_byte() << 8;
	degree |= read_next_byte();
	// degree to radiant
	float radiant = degree * 180/M_PI;
	set_m1_speed(degree < 0 ? 25 : -25);
	set_m2_speed(degree < 0 ? -25 : 25);
	delay_ms(radiant/ROBOT_CM_PER_SECOND);
	set_motors(0,0);
	
}

void rotate_right(){
	char degree = read_next_byte();
	set_motors(ROBOT_ROTATESPEED_IN_CMS*ROBOT_SPEEDSTEP+1, -ROBOT_ROTATESPEED_IN_CMS*ROBOT_SPEEDSTEP);
	delay_ms(ROBOT_ROTATE_DAMPING*(255/(ROBOT_SPEEDSTEP*ROBOT_ROTATESPEED_IN_CMS))*degree * ROBOT_DISTANCE_ONE_DEGREE * 10);
	set_motors(0,0);
}

void rotate_left(){
	char degree = read_next_byte();
	set_motors(-ROBOT_ROTATESPEED_IN_CMS*ROBOT_SPEEDSTEP, ROBOT_ROTATESPEED_IN_CMS*ROBOT_SPEEDSTEP+1);
	delay_ms(ROBOT_ROTATE_DAMPING*(255/(ROBOT_SPEEDSTEP*ROBOT_ROTATESPEED_IN_CMS))*degree * ROBOT_DISTANCE_ONE_DEGREE * 10);
	set_motors(0,0);
}

void print_number() {
	lcd_goto_xy(0,0);
	clear();
	uint16_t num = read_next_byte();
	num |= read_next_byte() << 8;
	print_long(num);
}

void print_text(){
	lcd_goto_xy(0,0);
	clear();

	unsigned char string_length = read_next_byte();

	unsigned char i;
	for(i=0;i<string_length && i < 8;i++)
	{
		unsigned char character;
		character = read_next_byte();
		
		if(character == '\n') {
			// Write to the next line
			break;			
		}

		print_character(character);
	}
	
	// Write to second line
	lcd_goto_xy(0,1);
	
	for(;i<string_length && i < 16;i++)
	{
		unsigned char character;
		character = read_next_byte();

		print_character(character);
	}
}

/////////////////////////////////////////////////////////////////////

int main()
{
	pololu_3pi_init(2000);
	play_mode(PLAY_CHECK);
	clear();

	init_USART();
	serial_receive_ring(buffer, 100);

	while(1)
	{
		// wait for a command
		char command = read_next_byte();
		
		switch(command)
		{
			case READ_RS_SENSORS:{
				get_RS_sensor_data();
				break;
			}
			case MOVE_FORWARD:{
				move_forward();
				break;
			}
			case MOVE_BACKWARD:{
				move_backward();
				break;
			}
			case ROTATE:{
				rotate();
				break;
			}
			case ROTATE_RIGHT:{
				rotate_right();
				break;
			}
			case ROTATE_LEFT:{
				rotate_left();
				break;
			}
			case PRINT:{
				print_text();
				break;
			}
			case PRINT_NUMBER:{
				print_number();
				break;
			}
			default: {
				clear();
				lcd_goto_xy(0,0);
				print("Bad cmd");
				lcd_goto_xy(0,1);
				print_hex_byte(command);

				play("o7l16crc");
				continue;
			}
		}
	}
}